﻿using System;
using System.Diagnostics;

/// <summary>
/// Summary description for SPCToDecimalDegrees
/// </summary>
public class SPCToDecimalDegrees
{
    public SPCToDecimalDegrees(double easting, double falseEasting, double northing, double falseNorthing, double centralMedian, double originProjection, double scaleFactor)
    {
        /*
         CM=SPCC(IZ,1)/RAD
        FE=SPCC(IZ,2)
        OR=SPCC(IZ,3)/RAD
        SF=1.D0-1.D0/SPCC(IZ,4)
        FN=SPCC(IZ,5)
         
         T(X,Y)=X+Y/60.D0
         
         SPCC(85,1)=T(106.D0,15.D0)
      SPCC(85,2)=500000.D0
      SPCC(85,3)=31.D0
      SPCC(85,4)=10000.D0
      SPCC(85,5)=0.D0
          
         */

        //double spcc1 = (106d + 15d / 60d);
        //double spcc2 = 500000d;
        //double spcc3 = 31d;
        //double spcc4 = 10000d;
        //double spcc5 = 0d;

        //double cm = spcc1 / rad;
        //double or = spcc3 / rad;
        //double sf = 1d - 1d / spcc4;


        //double so = computeConstants(sf, or);


        //convert((northing * (1200d / 3937d)), spcc5, (easting * (1200d / 3937d)), spcc2, cm, so, sf);


        double cm = centralMedian / rad;
        double or = originProjection / rad;


        double so = computeConstants(scaleFactor, or);


        convert((northing * (1200d / 3937d)), falseNorthing * (1200d / 3937d), (easting * (1200d / 3937d)), falseEasting * (1200d / 3937d), cm, so, scaleFactor);

    }

    public double Latitude
    {
        get;
        protected set;
    }

    public double Longitude
    {
        get;
        protected set;
    }

    /// <summary>
    /// ***          TRANSVERSE MERCATOR PROJECTION               ***
    /// *** CONVERSION OF GRID COORDS TO GEODETIC COORDS
    /// *** REVISED SUBROUTINE OF T. VINCENTY  FEB. 25, 1985
    /// ************** SYMBOLS AND DEFINITIONS ***********************
    /// *** ER IS THE SEMI-MAJOR AXIS FOR GRS-80
    /// *** SF IS THE SCALE FACTOR AT THE CM
    /// *** SO IS THE MERIDIANAL DISTANCE (TIMES THE SF) FROM THE
    /// ***       EQUATOR TO SOUTHERNMOST PARALLEL OF LAT. FOR THE ZONE
    /// *** R IS THE RADIUS OF THE RECTIFYING SPHERE
    /// *** U0,U2,U4,U6,V0,V2,V4,V6 ARE PRECOMPUTED CONSTANTS FOR
    /// ***   DETERMINATION OF MERIDIANAL DIST. FROM LATITUDE
    /// *** OR IS THE SOUTHERNMOST PARALLEL OF LATITUDE FOR WHICH THE
    /// ***       NORTHING COORD IS ZERO AT THE CM
    /// **************************************************************
    /// </summary>
    /// CALL TCONPC (SF,OR,EPS,R,SO,V0,V2,V4,V6,ER,ESQ)
    private double computeConstants(double scaleFactor, double originProjection)
    {
        double cosor = Math.Cos(originProjection);
        double omo = originProjection + Math.Sin(originProjection) * cosor * (u0 + u2 * cosor + u4 * Math.Pow(cosor, 4) + u6 * Math.Pow(cosor, 6));

        //return so;
        return scaleFactor * r * omo;
    }
    /// <summary>
    ///     ***          TRANSVERSE MERCATOR PROJECTION               ***
    ///*** CONVERSION OF GRID COORDS TO GEODETIC COORDS
    ///*** REVISED SUBROUTINE OF T. VINCENTY  FEB. 25, 1985
    ///************** SYMBOLS AND DEFINITIONS ***********************
    ///*** LATITUDE POSITIVE NORTH, LONGITUDE POSITIVE WEST.  ALL
    ///***          ANGLES ARE IN RADIAN MEASURE.
    ///*** LAT,LON ARE LAT. AND LONG. RESPECTIVELY
    ///*** N,E ARE NORTHING AND EASTING COORDINATES RESPECTIVELY
    ///*** K IS POINT SCALE FACTOR
    ///*** ER IS THE SEMI-MAJOR AXIS FOR GRS-80
    ///*** ESQ IS THE SQUARE OF THE 1ST ECCENTRICITY
    ///*** E IS THE 1ST ECCENTRICITY
    ///*** CM IS THE CENTRAL MERIDIAN OF THE PROJECTION ZONE
    ///*** FE IS THE FALSE EASTING VALUE AT THE CM
    ///*** CONV IS CONVERGENCE
    ///*** EPS IS THE SQUARE OF THE 2ND ECCENTRICITY
    ///*** SF IS THE SCALE FACTOR AT THE CM
    ///*** SO IS THE MERIDIANAL DISTANCE (TIMES THE SF) FROM THE
    ///***       EQUATOR TO SOUTHERNMOST PARALLEL OF LAT. FOR THE ZONE
    ///*** R IS THE RADIUS OF THE RECTIFYING SPHERE
    ///*** U0,U2,U4,U6,V0,V2,V4,V6 ARE PRECOMPUTED CONSTANTS FOR
    ///***   DETERMINATION OF MERIDIANAL DIST. FROM LATITUDE
    ///***
    ///*** THE FORMULA USED IN THIS SUBROUTINE GIVES GEODETIC ACCURACY
    ///*** WITHIN ZONES OF 7 DEGREES IN EAST-WEST EXTENT.  WITHIN STATE
    ///*** TRANSVERSE MERCATOR PROJECTION ZONES, SEVERAL MINOR TERMS OF
    ///*** THE EQUATIONS MAY BE OMMITTED (SEE A SEPARATE NGS PUBLICATION).
    ///*** IF PROGRAMMED IN FULL, THE SUBROUTINE CAN BE USED FOR
    ///*** COMPUTATIONS IN SURVEYS EXTENDING OVER TWO ZONES.
    ///***********************************************************************
    /// </summary>
    /// CALL TMGEOD(NORTH,EAST,LAT,LON,EPS,CM,FE,SF,SO,R,V0,V2,V4,V6,FN,ER,ESQ,CONV,KP)
    private void convert(double northing, double falseNorthing, double easting, double falseEasting, double centralMeridian, double so, double scaleFactor)
    {
        double om = (northing - falseNorthing + so) / (r * scaleFactor);
        double cosom = Math.Cos(om);
        double foot = om + Math.Sin(om) * cosom * (v0 + v2 * cosom * cosom + v4 * Math.Pow(cosom, 4) + v6 * Math.Pow(cosom, 6));

        double sinf = Math.Sin(foot);
        double cosf = Math.Cos(foot);

        double tn = sinf / cosf;

        double ts = tn * tn;

        double ets = eps * cosf * cosf;

        double rn = er * scaleFactor / Math.Sqrt(1d - esq * sinf * sinf);

        double q = (easting - falseEasting) / rn;

        double qs = q * q;

        double b2 = -tn * (1d + ets) / 2d;

        double b4 = -(5d + 3d * ts + ets * (1d - 9d * ts) - 4d * ets * ets) / 12d;

        double b6 = (61d + 45d * ts * (2d + ts) + ets * (46d - 252d * ts - 60d * ts * ts)) / 360d;

        double b1 = 1d;
        double b3 = -(1d + ts + ts + ets) / 6d;
        double b5 = (5d + ts * (28d + 24d * ts) + ets * (6d + 8d * ts)) / 120d;
        double b7 = -(61d + 662d * ts + 1320d * ts * ts + 720d * Math.Pow(ts, 3)) / 5040d;

        this.Latitude = toDD(foot + b2 * qs * (1d + qs * (b4 + b6 * qs)));
        double l = b1 * q * (1d + qs * (b3 + qs * (b5 + b7 * qs)));
        this.Longitude = toDD(-l / cosf + centralMeridian);



        /*

              IMPLICIT DOUBLE PRECISION(A-H,K-Z)

      OM=(N-FN+SO)/(R*SF)
      COSOM=DCOS(OM)
      FOOT=OM+DSIN(OM)*COSOM*(V0+V2*COSOM*COSOM+V4*COSOM**4+
     &     V6*COSOM**6)
      SINF=DSIN(FOOT)
      COSF=DCOS(FOOT)
      TN=SINF/COSF
      TS=TN*TN
      ETS=EPS*COSF*COSF
      RN=ER*SF/DSQRT(1.D0-ESQ*SINF*SINF)
      Q=(E-FE)/RN
      QS=Q*Q
      B2=-TN*(1.D0+ETS)/2.D0
      B4=-(5.D0+3.D0*TS+ETS*(1.D0-9.D0*TS)-4.D0*ETS*ETS)/12.D0
      B6=(61.D0+45.D0*TS*(2.D0+TS)+ETS*(46.D0-252.D0*TS-
     &    60.D0*TS*TS))/360.D0
      B1=1.D0
      B3=-(1.D0+TS+TS+ETS)/6.D0
      B5=(5.D0+TS*(28.D0+24.D0*TS)+ETS*(6.D0+8.D0*TS))/120.D0
      B7=-(61.D0+662.D0*TS+1320.D0*TS*TS+720.D0*TS**3)/5040.D0
      LAT=FOOT+B2*QS*(1.D0+QS*(B4+B6*QS))
      L=B1*Q*(1.D0+QS*(B3+QS*(B5+B7*QS)))
      LON=-L/COSF+CM
C*********************************************************************
C     COMPUTE CONVERENCE AND SCALE FACTOR
      FI=LAT
      LAM = LON
      SINFI=SIN(FI)
      COSFI=COS(FI)
      L1=(LAM-CM)*COSFI
      LS=L1*L1
C
C*** CONVERGENCE
      C1=-TN
      C3=(1.+3.*ETS+2.*ETS**2)/3.
      C5=(2.-TS)/15.
      CONV=C1*L1*(1.+LS*(C3+C5*LS))
C
C*** POINT SCALE FACTOR
      F2=(1.+ETS)/2.
      F4=(5.-4.*TS+ETS*( 9.-24.*TS))/12.
      KP=SF*(1.+F2*LS*(1.+F4*LS))

      RETURN
      END


      */
    }

    private double toDD(double rad)
    {
        double rhosec = 2.062648062471d * Math.Pow(10, 5);
        double sec = rad * rhosec;
        double idg = sec / 3600d;
        sec = sec - idg * 3600;
        double min = sec / 60d;

        if (!((60d - Math.Abs(sec)) > 5 * Math.Pow(10, -6)))
        {
            sec = sec - sign(60d, sec);
            min = min + sign(1, min);
        }

        if (!(Math.Abs(min) < 60))
        {
            min = min - sign(60, min);
            idg = idg + sign(1, idg);
        }

        min = Math.Abs(min);
        sec = Math.Abs(sec);

        if (!(rad < 0))
        {
            if (idg.Equals(0))
            {
                min = -min;
            }

            if (idg.Equals(0) && min.Equals(0))
            {
                sec = -sec;
            }
        }


        return idg;
        /*
         REAL*8 RAD,SEC,RHOSEC
      DATA RHOSEC/2.062648062471D05/
      SEC=RAD*RHOSEC
      IDG=SEC/3600.D0
      SEC=SEC-DBLE(IDG*3600)
      MIN=SEC/60.D0
      SEC=SEC-DBLE(MIN*60)
      IF((60.D0-DABS(SEC)).GT.5.D-6) GO TO 100
      SEC=SEC-DSIGN(60.D0,SEC)
      MIN=MIN+ISIGN(1,MIN)
  100 IF(IABS(MIN).LT.60) GO TO 101
      MIN=MIN-ISIGN(60,MIN)
      IDG=IDG+ISIGN(1,IDG)
  101 MIN=IABS(MIN)
      SEC=DABS(SEC)
      IF(RAD.GE.0.D0) GO TO 102
      IF(IDG.EQ.0) MIN=-MIN
      IF(IDG.EQ.0.AND.MIN.EQ.0)SEC=-SEC
  102 RETURN
      END
    
         */
    }
    /// <summary>
    /// Returns `ABS(A)*s', where s is +1 if `B.GE.0', -1 otherwise. 
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <returns></returns>
    private double sign(double a, double b)
    {
        if (b > 0)
        {
            return Math.Abs(a) * +1;
        }
        else
        {
            return Math.Abs(a) * -1;
        }
    }

    private int sign(int a, int b)
    {
        if (b > 0)
        {
            return Math.Abs(a) * +1;
        }
        else
        {
            return Math.Abs(a) * -1;
        }
    }


    /* Globals
         
RF=298.257222101D0
F=1.D0/RF
ESQ=(F+F-F*F)
ER=6378137.D0
         
 */
    private const double pr = (1d - f) * er;
    private const double en = (er - pr) / (er + pr);

    //These are kind of lame, but it makes transposing the FORTRAN code easier.
    private const double en2 = en * en;
    private const double en3 = en * en * en;
    private const double en4 = en2 * en2;

    private const double c2 = -3d * en / 2d + 9d * en3 / 16d;
    private const double c4 = 15d * en2 / 16d - 15d * en4 / 32d;
    private const double c6 = -35d * en3 / 48d;
    private const double c8 = 315d * en4 / 512d;

    private const double u0 = 2d * (c2 - 2d * c4 + 3d * c6 - 4d * c8);
    private const double u2 = 8d * (c4 - 4d * c6 + 10d * c8);
    private const double u4 = 32d * (c6 - 6d * c8);
    private const double u6 = 128d * c8;

    private const double cc2 = 3d * en / 2d - 27d * en3 / 32d;
    private const double cc4 = 21d * en2 / 16d - 55d * en4 / 32d;
    private const double cc6 = 151d * en3 / 96d;
    private const double cc8 = 1097d * en4 / 512d;

    private const double v0 = 2d * (cc2 - 2d * cc4 + 3d * cc6 - 4d * cc8);
    private const double v2 = 8d * (cc4 - 4d * cc6 + 10d * cc8);
    private const double v4 = 32d * (cc6 - 6d * cc8);
    private const double v6 = 128d * cc8;

    private const double r = er * (1d - en) * (1d - en * en) * (1d + 2.25d * en * en + (225d / 64d) * en4);

    private const double f = 1.0d / 298.257222101d;
    private const double eps = esq / (1d - esq);
    private const double esq = f + f - f * f;
    private const double er = 6378137d;
    private const double rad = 180.0 / Math.PI;
}
